/*
 * @Author: chenbolin@msn.cn
 * @Date: 2023-03-20 17:14:37
 * @LastEditors: chenbolin@msn.cn
 * @LastEditTime: 2023-10-26 14:26:05
 * @FilePath: \Remote\src\App\MotionCtrl.c
 * @Description:
 *
 * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
 */
#include "MotionCtrl.h"
#include "Button.h"

typedef enum
{
    MOTION_CTRL_STATE_STAY = 0,
    MOTION_CTRL_STATE_RAISE,
    MOTION_CTRL_STATE_DESCENT,
} MotionCtrlState_t;

static float g_lastActualValue[SYS_VALVE_CH_COUNT][2];
static uint16_t g_targetCurrent[SYS_VALVE_CH_COUNT][2];

uint16_t MotionCtrlGetTargetCurrent(SysValveChannel_t ch, SysJoystickDirect_t dir)
{
    // if (SysIsEmergencyStop()) // 急停按下
    // {
    //     return 0;
    // }
    g_targetCurrent[ch][dir] = SysJoystickPercentToCurrent(ch, dir);
    return g_targetCurrent[ch][dir];
}

uint16_t MCTargetCurrent(SysValveChannel_t ch, SysJoystickDirect_t dir)
{
    return g_targetCurrent[ch][dir];
}

uint16_t MotionCtrlGetLastActualValue(SysValveChannel_t ch, SysJoystickDirect_t dir)
{
    return (uint16_t)g_lastActualValue[ch][dir];
}

static uint16_t calcActualValue(SysValveChannel_t ch, SysJoystickDirect_t dir, uint16_t target, float step, MotionCtrlState_t state)
{
    int diff = target - MotionCtrlGetLastActualValue(ch, dir);
    // float step;

    if (diff > 0)
    {
        g_lastActualValue[ch][dir] += (diff > step ? step : diff);
    }
    else if (diff < 0)
    {
        g_lastActualValue[ch][dir] -= (abs(diff) > step ? step : abs(diff));
    }

    return (uint16_t)g_lastActualValue[ch][dir];
}

uint16_t MTRealCurrent(SysValveChannel_t ch, SysJoystickDirect_t dir)
{
    MotionCtrlState_t ctrl = MOTION_CTRL_STATE_STAY;
//    static SysTime_t motorStopTime[2] = {0, 0};
    SysValveStep_t *valveStep = SysGetValveStep(ch, dir);
    SysSettings_t *st = SysSettings();
    uint16_t target;
    float step = 0;

    // if (SysIsEmergencyStop()) // 急停按下
    // {
    //     g_lastActualValue[ch][dir] = 0;
    //     return 0;
    // }

    target = MotionCtrlGetTargetCurrent(ch, dir);

    /*缓升缓降*/
    if (SysWorkMode() == SYS_WORK_MODE_NORMAL)
    {
        if (target > MotionCtrlGetLastActualValue(ch, dir)) // 升
        {
            if (MotionCtrlGetLastActualValue(ch, (SysJoystickDirect_t)!dir) == 0) // 反方向降低执行完成才执行升操作
            {
                if (MotionCtrlGetLastActualValue(ch, dir) < st->current[ch][dir].min)
                {
                    g_lastActualValue[ch][dir] = st->current[ch][dir].min; // 从最小值开始
                }
                ctrl = MOTION_CTRL_STATE_RAISE;
                step = (SysSpeedMode() == SYS_SPEED_MODE_FAST) ? valveStep->fast.raise : valveStep->slow.raise;
            }
            else
            {
                g_lastActualValue[ch][dir] = 0;
                target = 0;
            }
        }
        else if (target < MotionCtrlGetLastActualValue(ch, dir)) // 降
        {
            if (MotionCtrlGetLastActualValue(ch, dir) <= st->current[ch][dir].min)
            {
                g_lastActualValue[ch][dir] = 0; // 低于最小值为0
            }

            ctrl = MOTION_CTRL_STATE_DESCENT;
            step = (SysSpeedMode() == SYS_SPEED_MODE_FAST) ? valveStep->fast.descent : valveStep->slow.descent;
        }
        return calcActualValue(ch, dir, target, step, ctrl);
    }
    return target;
}

void MotionCtrlReset(void)
{
    memset(g_lastActualValue, 0, sizeof(g_lastActualValue));
}

void MotionCtrlInitialize(void)
{
    MotionCtrlReset();
}
